// motor.c

#include "motor.h"
#include "ecrt.h"
#include "forward_kinematics.h"
#include "igh.h"
#include "mqtt_comm.h"
#include "rs485.h"
#include "term.h"
#include <pthread.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

static const uint32_t GRATING_TARGET_VALUE = 103051500; // 理想光栅值
static const double GRATING_TO_PULSE_RATIO =
    4096.0 / 10735.0; // 光栅尺到电机脉冲的转换比例
// static const double GRATING_TO_ANGLE_RATIO = 1.01413755 / 10735.0;
// //光栅尺到角度的转换比例
extern double actual_theta6_angle;
extern double g_current_obj_pos[6];

static const int NORMAL_THETA1_MIN = 138720;
static const int NORMAL_THETA1_MAX = 261784;
// 引入命令队列
extern pthread_mutex_t cmdSet_mutex;
extern MotorSet cmdSet;

/* 转换比例 */
static const double POSITION_PULSE_PER_M = 1.0 / 50e-9; // 位移：1脉冲对应50nm
static const double ROTATIONAL_PULSE_PER_DEG_DD =
    1.0 / 0.001; // 力矩电机：1脉冲对应0.001°
static const double ROTATIONAL_PULSE_PER_DEG_ZOLIX =
    1.0 / 0.0002475921753; // 天瀚精密电机：1脉冲对应0.0002475921753
/* 各轴零点脉冲数 */
extern int ZERO_PULSE_THETA6; // 旋转关节6
extern int LIMIT_THETA6_MIN;
extern int LIMIT_THETA6_MAX;

extern int grating_init_done;

// static const int ZERO_PULSE_THETA6 = 0;   // 旋转关节6
static const int ZERO_PULSE_THETA5 = 71158;  // 旋转关节5
static const int ZERO_PULSE_D4 = 795823;     // 位移关节4
static const int ZERO_PULSE_D3 = 703125;     // 位移关节3
static const int ZERO_PULSE_D2 = 705678;     // 位移关节2
static const int ZERO_PULSE_THETA1 = 226148; // 旋转关节1
/**********************CStruct**********************************/
ec_pdo_entry_info_t slave_motor_pdo_entries[] = {
    {0x6040, 0x00, 16}, // 控制字 U16
    {0x6060, 0x00, 8},  // 操作模式 I8
    {0x607a, 0x00, 32}, // 目标位置 S32

    {0x6041, 0x00, 16}, // 状态字 U16
    {0x6061, 0x00, 8},  // 操作模式显示 I8
    {0x6064, 0x00, 32}, // 当前位置 S32
};

ec_pdo_info_t slave_motor_pdos[] = {
    {0x1600, 3, slave_motor_pdo_entries + 0}, // RXPDO
    {0x1a00, 3, slave_motor_pdo_entries + 3}, // TXPDO
};

ec_sync_info_t slave_motor_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_motor_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_motor_pdos + 1, EC_WD_DISABLE},
    {0xff}};
/**********************CStruct**********************************/

// 电机位置控制CSP模式
void csp(struct _SlaveConfig *slave_config, struct _Domain *domain,
         int target_pos, int motorID, int *state) {
  int8_t operation_mode_display =
      EC_READ_S8(domain->domain_pd +
                 motor_parm[motorID].operation_mode_display); // 读取状态字
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字

  // 检查是否有错误状态
  if (status & 0x08) { // 故障位检查
    printf("电机 %d 检测到故障，状态字: 0x%04X\n", motorID, status);
    EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                 0x80); // 发送故障复位
    *state = -1; // 设置为故障复位状态，下个周期重新开始
    return;
  }
  if (*state < 10) {
    if (*state == -1) {
      // 故障复位后的等待状态
      if ((status & 0x08) == 0) { // 故障位已清除
        *state = 0;               // 开始正常初始化
      }
      return;
    }
    if (*state == 0) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x06); // 重置电机
      if ((status & 0xFF) == 0x31) {
        *state = 1;
      }
    } else if (*state == 1) {
      EC_WRITE_S8(domain->domain_pd + motor_parm[motorID].operation_mode,
                  MODEL_CSP); // 设置操作模式
      if (operation_mode_display == MODEL_CSP) {
        *state = 2;
      }
    } else if (*state == 2) { //标记：模式切换位置的影响
      int actual_motor =
          EC_READ_S32(domain->domain_pd + motor_parm[motorID].current_pos);
      int target_motor =
          EC_READ_S32(domain->domain_pd + motor_parm[motorID].target_pos);
      EC_WRITE_S32(domain->domain_pd + motor_parm[motorID].target_pos,
                   actual_motor); // 设置目标位置
      int diff1 = target_motor - actual_motor;
      int diff2 = target_pos - actual_motor;
      if (-100 < diff1 && diff1 < 100 && -3000 < diff2 && diff2 < 3000) {
        *state = 3;
      }
    } else if (*state == 3) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x07); // 电机得电
      if ((status & 0xFF) == 0x33) {
        *state = 4;
      }
    } else if (*state == 4) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x0f); // 使能电机
      if ((status & 0xFF) == 0x37) {
        *state = 10;
      }
    }
  } else {
    EC_WRITE_S32(domain->domain_pd + motor_parm[motorID].target_pos,
                 target_pos); // 设置目标位置
  }
}
// 主电机控制函数
void motor_main(struct _SlaveConfig *slave_config, struct _Domain *domain) {
  // 上锁
  pthread_mutex_lock(&cmdSet_mutex);

  /**************************
   * 初始化光栅尺（只初始化一次）及确认电机编码器偏差和摆台范围，
   * **************************/
  if (!grating_init_done) {
    if (grating_scale_init() == 0) {
      printf("电机初始化过程中光栅尺初始化成功\n");
      // 1. 读取当前光栅尺的实际值
      uint32_t current_grating_value;
      int32_t pulse_adjustment = 0; // 初始化补偿值为0

      if (grating_scale_read_current_value(&current_grating_value) == 0) {
        printf("成功读取初始光栅尺数值: %u\n", current_grating_value);

        // 2. 计算光栅尺偏差 (使用有符号整数，因为差值可正可负)
        int32_t grating_diff =
            (int32_t)current_grating_value - (int32_t)GRATING_TARGET_VALUE;

        // 3. 根据偏差和转换比例，计算电机需要补偿的脉冲数
        pulse_adjustment = (int32_t)(grating_diff * GRATING_TO_PULSE_RATIO);

        printf("光栅尺目标: %u, 当前: %u, 偏差: %d\n", GRATING_TARGET_VALUE,
               current_grating_value, grating_diff);
        printf("计算出的电机补偿脉冲: %d\n", pulse_adjustment);

        LIMIT_THETA6_MIN = -40694 + pulse_adjustment;
        LIMIT_THETA6_MAX = 39827 + pulse_adjustment;

        ZERO_PULSE_THETA6 = pulse_adjustment;
        printf("已将零点脉冲偏差 %d 更新到 ZERO_PULSE_THETA6\n",
               ZERO_PULSE_THETA6);

      } else {
        printf("读取初始光栅尺数值失败，无法进行位置补偿\n");
        // 读取失败，可以考虑报错或进入错误状态
        g_robotStateData.taskState = 204; // 例如：光栅尺读取错误
      }
    } else {
      printf("电机初始化过程中光栅尺初始化失败\n");
    }
    grating_init_done = 1;
  }

  /************************** 读取电机状态 **************************/
  // 1. 读取各关节实际脉冲位置及当前光栅实际读数
  for (int i = 0; i < MOTOR_NUM; i++) {
    cmdSet.actual_pos[i] =
        EC_READ_S32(domain->domain_pd + motor_parm[i].current_pos);
  }

  // 打印第一个轴的电机脉冲值（索引0对应theta6）
  // printf("第一个轴(theta6)电机脉冲值: %d\n", cmdSet.actual_pos[5]);

  // 2. 脉冲转换为物理量 (关节空间)
  // g_robotStateData.PositionAxis[5] =
  //     (cmdSet.actual_pos[0] - ZERO_PULSE_THETA6) /
  //     ROTATIONAL_PULSE_PER_DEG_ZOLIX;
  g_robotStateData.PositionAxis[4] =
      (cmdSet.actual_pos[1] - ZERO_PULSE_THETA5) / ROTATIONAL_PULSE_PER_DEG_DD;
  g_robotStateData.PositionAxis[3] =
      (cmdSet.actual_pos[2] - ZERO_PULSE_D4) / POSITION_PULSE_PER_M;
  g_robotStateData.PositionAxis[2] =
      (cmdSet.actual_pos[3] - ZERO_PULSE_D3) / POSITION_PULSE_PER_M;
  g_robotStateData.PositionAxis[1] =
      (cmdSet.actual_pos[4] - ZERO_PULSE_D2) / POSITION_PULSE_PER_M;
  g_robotStateData.PositionAxis[0] =
      (cmdSet.actual_pos[5] - ZERO_PULSE_THETA1) / ROTATIONAL_PULSE_PER_DEG_DD;

  // ======================= 核心修改点 开始 =======================
  // 根据任务状态，为第6轴(theta6)选择最准确的角度数据源
  if (g_robotStateData.taskState == 1) {
    // 任务已完成，此时光栅精调也已结束。
    // 使用全局变量中存储的、由光栅尺计算出的最终精确角度。
    g_robotStateData.PositionAxis[5] = actual_theta6_angle;
  } else {
    // 任务正在执行中 (taskState == 0) 或处于其他状态。
    // 此时应使用电机编码器提供实时的、虽然精度稍低的角度。
    g_robotStateData.PositionAxis[5] =
        (cmdSet.actual_pos[0] - ZERO_PULSE_THETA6) /
        ROTATIONAL_PULSE_PER_DEG_ZOLIX; // 使用编码器的转换比例
  }
  // ======================= 核心修改点 结束 =======================

  // // 3. 正运动学计算 (关节→末端)
  // forward_kinematics_from_pulses(
  //     cmdSet.actual_pos[5], cmdSet.actual_pos[4], cmdSet.actual_pos[3],
  //     cmdSet.actual_pos[2], cmdSet.actual_pos[1], cmdSet.actual_pos[0],
  //     g_current_obj_pos, // 使用全局 obj_pos
  //     &g_robotStateData.PositionXYZ[0], &g_robotStateData.PositionXYZ[1],
  //     &g_robotStateData.PositionXYZ[2], &g_robotStateData.PositionXYZ[3],
  //     &g_robotStateData.PositionXYZ[4], &g_robotStateData.PositionXYZ[5]);

  // 3. 正运动学计算 (关节→末端)
  // 【重要】确保这里调用的是接收物理量(角度/米)的 ideal_DH_FK 函数
  ideal_DH_FK(
      g_robotStateData.PositionAxis[0], // theta1_deg
      g_robotStateData.PositionAxis[1], // d2_m
      g_robotStateData.PositionAxis[2], // d3_m
      g_robotStateData.PositionAxis[3], // d4_m
      g_robotStateData.PositionAxis[4], // theta5_deg
      g_robotStateData.PositionAxis[5], // theta6_deg (此值已根据状态动态选择)
      g_current_obj_pos, &g_robotStateData.PositionXYZ[0],
      &g_robotStateData.PositionXYZ[1], &g_robotStateData.PositionXYZ[2],
      &g_robotStateData.PositionXYZ[3], &g_robotStateData.PositionXYZ[4],
      &g_robotStateData.PositionXYZ[5]);

  // /* --- Debug: 在轴控制（taskType == 1）时打印末端位姿 --- */
  // /* g_robotControlData.taskType == 1 表示 AxisControl（关节/轴控制） */
  // if (g_robotControlData.taskType == 1) {
  //   printf("[DEBUG] AxisControl FK (units: μm, deg)\n");
  //   printf("  EndPose: X=%.6f μm, Y=%.6f μm, Z=%.6f μm, Rx=%.6f°, Ry=%.6f°, "
  //          "Rz=%.6f°\n",
  //          g_robotStateData.PositionXYZ[0], g_robotStateData.PositionXYZ[1],
  //          g_robotStateData.PositionXYZ[2], g_robotStateData.PositionXYZ[3],
  //          g_robotStateData.PositionXYZ[4], g_robotStateData.PositionXYZ[5]);

  //   /* 同时打印关节空间（以便比对）：
  //      PositionAxis[] 单位：角度为 °，位移为 m（注意 d2/d3/d4 在此为 m） */
  //   printf("  Joints: th1=%.6f°, d2=%.6f m, d3=%.6f m, d4=%.6f m, th5=%.6f°,
  //   "
  //          "th6=%.6f°\n",
  //          g_robotStateData.PositionAxis[0],
  //          g_robotStateData.PositionAxis[1],
  //          g_robotStateData.PositionAxis[2],
  //          g_robotStateData.PositionAxis[3],
  //          g_robotStateData.PositionAxis[4],
  //          g_robotStateData.PositionAxis[5]);
  // }

  /************************** 初始化电机 **************************/
  static int init_flag = 0;
  if (init_flag < 10) {
    if (init_flag == 0) {
      int8_t operation_mode_display = 0;
      bool motor_start_flag = true;
      for (int i = 1; i < MOTOR_NUM; i++) {
        operation_mode_display =
            EC_READ_S8(domain->domain_pd +
                       motor_parm[i].operation_mode_display); // 读取状态字
        // printf("operation_mode_display: %d,id: %d\n", operation_mode_display,
        // i);
        if (operation_mode_display != 1 && operation_mode_display != 8) {
          motor_start_flag = false; // 只要有一个不是 1，就设为 false
          break;                    // 直接退出循环，提高效率
        }
      }
      if (motor_start_flag) {
        init_flag = 1;
      }
    }
    if (init_flag == 1) { // 设置目标位置
      int theta1_init =
          EC_READ_S32(domain->domain_pd + motor_parm[5].current_pos);
      if (theta1_init < NORMAL_THETA1_MIN ||
          theta1_init > NORMAL_THETA1_MAX) { /////标记
        g_robotStateData.taskState = 203;    // 范围错误
      }
      // 4. 设置所有电机的初始目标位置为当前位置
      // -------------------  修改开始 -------------------
      // for (int i = 0; i < MOTOR_NUM; i++) {
      //   if (i == 0) { // 如果是第六轴 (索引为0)
      //     // 修正：不读取它的相对编码器值，
      //     // 而是直接使用光栅尺校正后的零点脉冲作为它的“当前位置”
      //     cmdSet.plan_pos[i] = ZERO_PULSE_THETA6;
      //   } else {
      //     // 其他轴保持原有逻辑 (假设它们是绝对编码器或已正确归零)
      //     cmdSet.plan_pos[i] =
      //         EC_READ_S32(domain->domain_pd + motor_parm[i].current_pos);
      //   }
      // }
      // -------------------  修改结束 -------------------
      // 4. 设置所有电机的初始目标位置为当前位置
      for (int i = 0; i < MOTOR_NUM; i++) {
        cmdSet.plan_pos[i] =
            EC_READ_S32(domain->domain_pd + motor_parm[i].current_pos);
      }
      init_flag = 2;
    } else if (init_flag == 2) { // 执行第一次初始化
      for (int i = 0; i < MOTOR_NUM; i++) {
        csp(slave_config, domain, cmdSet.plan_pos[i], i, &cmdSet.state[i]);
      }
      if (cmdSet.state[0] == 10 && cmdSet.state[1] == 10 &&
          cmdSet.state[2] == 10 && cmdSet.state[3] == 10 &&
          cmdSet.state[4] == 10 && cmdSet.state[5] == 10) {
        init_flag = 10;
        g_robotStateData.taskState = 1; //初始化完成,可以接收任务
        memcpy(cmdSet.plan_pos, cmdSet.actual_pos, sizeof(cmdSet.plan_pos));
      }
    }
  }

  // /************************** 电机控制 **************************/
  // 电机失能（只有急停和错误状态才失能）
  if (g_robotStateData.taskState > 200) {
    for (int i = 0; i < MOTOR_NUM; i++) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[i].ctrl_word,
                   0x06); // 电机失能
    }
    return;
  }
  // 暂停状态下保持电机使能但不改变目标位置
  if (g_robotStateData.taskState == 5) {
    // 暂停状态：电机保持使能，目标位置不变
    updateStatues(&cmdSet);
    for (int i = 0; i < MOTOR_NUM; i++) {
      csp(slave_config, domain, cmdSet.plan_pos[i], i, &cmdSet.state[i]);
    }
    pthread_mutex_unlock(&cmdSet_mutex);
    return;
  }
  updateStatues(&cmdSet); // 切换模式时，只重置卓立汉光电机
  // 电机控制
  for (int i = 0; i < MOTOR_NUM; i++) {
    // 所有电机都统一使用CSP模式
    csp(slave_config, domain, cmdSet.plan_pos[i], i, &cmdSet.state[i]);
  }
  //解锁
  pthread_mutex_unlock(&cmdSet_mutex);
}
